# -*- encoding=utf8 -*-
import sys
import numpy as np
import cv2 as cv
import random
import copy

np.random.seed(951105)

TIME = [0]
CARDISTRIBUTION = [0, 0, 0]
CARNAMESPACE, ROADNAMESPACE, CROSSNAMESPACE = [], [], []
CROSSDICT, CARDICT, ROADDICT = {}, {}, {}
ROUTEMAP = {}


class CAR(object):
    def __init__(self, id_, from_, to_, speed_, planTime_):
        # **** statistic parameters ****#
        self.id_, self.from_, self.to_, self.speed_, self.planTime_ = id_, from_, to_, speed_, planTime_
        self.carColor = [int(value) for value in np.random.random_integers(0, 255, [3])]
        # **** dynamic parameters ****#
        self.state, self.x, self.y = 0, 0, 0
        self.presentRoad, self.nextCrossId = None, self.from_
        self.deltaX, self.deltaY = 0, 0
        self.wait = False
        self.route, self.routeIndex = None, None

    #
    # simulate initialization
    #
    def simulateInit(self, route):
        #self.planTime_, self.route, self.routeIndex = planTime, route, 0
        self.route=route
        self.routeIndex = 0

    #
    # dynamic param update
    #
    def updateDynamic(self, state, x=None, y=None, presentRoad=None, roadSpeed=None, nextCrossId=None):
        # car not in carport of car is ready to go
        if self.state != 0 or presentRoad is not None:
            self.state = state
        if presentRoad is not None and self.state != 0 and self.routeIndex < self.route.__len__():  # routeIndex & self.route.len 什么意思
            self.routeIndex += 1
        self.x = x if x is not None else self.x
        self.y = y if y is not None else self.y
        self.presentRoad = presentRoad if presentRoad is not None else self.presentRoad
        if nextCrossId is not None:
            self.nextCrossId = nextCrossId
            toX, toY = CROSSDICT[self.to_].__loc__()
            nextCrossX, nextCrossY = CROSSDICT[nextCrossId].__loc__()
            self.deltaX, self.deltaY = toX - nextCrossX, toY - nextCrossY

    # show statistic parameters
    def __id__(self):
        return self.id_

    def __from__(self):
        return self.from_

    def __to__(self):
        return self.to_

    def __speed__(self):
        return self.speed_

    def __planTime__(self):
        return self.planTime_

    def __carColor__(self):
        return self.carColor

    #
    # show dynamic parameters
    #
    def __state__(self):
        return self.state

    def __x__(self):
        return self.x

    def __y__(self):
        return self.y

    def __presentRoad__(self):
        return self.presentRoad

    def __nextCrossId__(self):
        return self.nextCrossId

    def __deltaX__(self):
        return self.deltaX

    def __deltaY__(self):
        return self.deltaY

    def __wait__(self):
        return self.wait

    def __route__(self):
        return self.route

    def __routeIndex__(self):
        return self.routeIndex

    #
    # show some important info
    #
    def __v__(self):
        return min(self.speed_, ROADDICT[self.presentRoad].__speed__())

    def __distance__(self):
        return abs(self.deltaX) + abs(self.deltaY)

    def __nextRoad__(self):
        try:
            return self.route[self.routeIndex]
        except:
            return -1


class ROAD(object):
    def __init__(self, id_, length_, speed_, channel_, from_, to_, isDuplex_):
        # **** statistic parameters ****#
        self.id_, self.length_, self.speed_, self.channel_, self.from_, self.to_, self.isDuplex_ = \
            id_, length_, speed_, channel_, from_, to_, isDuplex_
        self.carCapcity = self.channel_ * self.length_
        # **** dynamic parameters ****#
        # absolute bucket
        # #type:dict （length, channel）
        self.forwardBucket = {i: [None for j in range(self.channel_)] for i in range(self.length_)}
        self.backwardBucket = {i: [None for j in range(self.channel_)] for i in
                               range(self.length_)} if self.isDuplex_ else None
        self.fx, self.fy, self.bx, self.by, self.forwardNum, self.backwardNum = [0], [0], [0], [0], [0], [0]
        self.forwardDone, self.backwardDone = [False], [False]
        # relative bucket
        self.provideBucket, self.receiveBucket = None, None
        self.px, self.py, self.provideNum, self.receiveNum = None, None, None, None
        self.provideDone = None
        # fx fy bx by px py 分别是 forward backward provide 的车辆坐标

    #
    # determine relative bucket
    # 闹呢，只有下面setBucket调用，pr只为'provide'
    # 路是从from到to的一个方向。如果crossId == from，则为backward
    # 相反，为forward
    def chooseAbsoluteBucket(self, crossId, pr):
        if crossId == self.from_ and pr == 'provide':
            return 'backward'
        elif crossId == self.from_ and pr == 'receive':
            return 'forward'
        elif crossId == self.to_ and pr == 'provide':
            return 'forward'
        elif crossId == self.to_ and pr == 'receive':
            return 'backward'
        else:
            print("Keywords mistake in CAR.chooseAbsoluteBucket()")

    # 搞不懂provideBucket、receiveBucket与forwardBucket、backwardBucket什么关系
    # 我理解，(x)表示路口 (--->)表示道路和方向
    # from(x)--->to 表示'backward'
    # from--->to(x) 表示'forward' ，即靠近路口的方向称为forward，远离的backward
    # 在forward下，道路为该路口提供车辆，于是provideBucket = forwardBucket
    # 在backward下，道路接收该路口来的车辆，于是receiveBucket = backBucket
    def setBucket(self, crossId):
        bucket = self.chooseAbsoluteBucket(crossId, 'provide')
        if bucket == 'forward':
            self.provideBucket, self.px, self.py, self.provideDone, self.provideNum = \
                [self.forwardBucket, self.fx, self.fy, self.forwardDone, self.forwardNum]
            if self.isDuplex_:
                self.receiveBucket, self.receiveNum = \
                    self.backwardBucket, self.backwardNum
            else:
                self.receiveBucket, self.receiveNum = None, None
        else:
            self.receiveBucket, self.receiveNum = \
                self.forwardBucket, self.forwardNum
            if self.isDuplex_:
                self.provideBucket, self.px, self.py, self.provideDone, self.provideNum = \
                    self.backwardBucket, self.bx, self.by, self.backwardDone, self.backwardNum
            else:
                self.provideBucket, self.px, self.py, self.provideDone, self.provideNum = \
                    None, None, None, None, None

    #
    # stepInitial
    #
    # stepInit 道路上车辆到初始化
    # 扫描道路上的所有车辆，并更新车辆状态（updateDynamic）
    # 然后用moveInChannel对道路上对车辆进行行驶
    #
    def stepInit(self):
        # dynamic param initialization
        self.fx, self.fy, self.bx, self.by = [0], [0], [0], [0]
        self.forwardDone, self.backwardDone = [False], [False]
        self.provideBucket, self.receiveBucket = None, None
        self.px, self.py, self.provideNum, self.receiveNum = None, None, None, None
        self.provideDone = None
        # car state initialization
        for i in range(self.length_):
            for j in range(self.channel_):
                if self.forwardBucket[i][j] is not None:
                    car = CARDICT[self.forwardBucket[i][j]]
                    car.updateDynamic(state=1)
                if self.isDuplex_:
                    if self.backwardBucket[i][j] is not None:
                        car = CARDICT[self.backwardBucket[i][j]]
                        car.updateDynamic(state=1)
        # first step
        for channel in range(self.channel_):
            self.moveInChannel(self.forwardBucket, channel)
            if self.isDuplex_:
                self.moveInChannel(self.backwardBucket, channel)

    #
    # function for bucket action
    #
    # moveInChannel : 进行道路上车辆的行驶，如果前车是终止状态，后车也前进到终止状态
    # if: 车辆为终止状态，则将车辆记录
    # elif: i-v > previousCar 表示当前车辆以v速度行驶，不会撞上前车。由此猜测0为道路末尾
    # elif: 前车为终止状态，判断当前车辆是否处于前车正后方，如果不在，将行驶到前车正后方
    # else: 车辆不动，判断为等待状态。
    #
    def moveInChannel(self, bucket, channel):
        # car state: 0,1,2,3 in carport,waiting,finishing,end
        # set guard
        previousCar, previousState = -1, 1
        for i in range(self.length_):
            if bucket[i][channel] is not None:
                car = CARDICT[bucket[i][channel]]
                v = car.__v__()
                if car.__state__() == 2:
                    previousCar, previousState = i, 2
                    continue
                elif i - v > previousCar:
                    bucket[i - v][channel] = bucket[i][channel]
                    bucket[i][channel] = None
                    previousCar, previousState = i - v, 2
                    car.updateDynamic(state=2, x=previousCar)
                elif previousState == 2:
                    if previousCar + 1 != i:
                        bucket[previousCar + 1][channel] = bucket[i][channel]
                        bucket[i][channel] = None
                    previousCar, previousState = previousCar + 1, 2
                    car.updateDynamic(state=2, x=previousCar)
                else:
                    previousCar, previousState = i, 1

    # findCar
    # 找到该车道上车的所处位置，找不到返回-1
    #
    def findCar(self, st, end, channel, bucket):
        # find car backward
        for i in range(end, st, -1):
            if bucket[i][channel] is not None:
                return i
        return -1

    #
    # provide car
    #
    # firstPriorityCar:第一优先级车辆
    # 什么是provide car ?
    #
    def firstPriorityCar(self):
        if self.provideBucket is None:
            print("Please do CAR.setBucket() first!")
        while True:
            if self.px[0] == self.length_:
                break
            carId = self.provideBucket[self.px[0]][self.py[0]]
            if carId is not None and CARDICT[carId].__state__() != 2:
                car = CARDICT[carId]
                left = car.__v__()
                # speed enough and no front car
                if left > self.px[0] and self.findCar(-1, self.px[0] - 1, self.py[0], self.provideBucket) == -1:
                    return self.provideBucket[self.px[0]][self.py[0]]
            if self.py[0] == self.channel_ - 1:
                self.px[0], self.py[0] = self.px[0] + 1, 0
            else:
                self.py[0] += 1
        self.provideDone[0] = True
        return -1

    def firstPriorityCarAct(self, action):
        if self.provideBucket is None:
            print("Please do CAR.setBucket() first!")
        if action == 0:
            self.provideBucket[self.px[0]][self.py[0]] = None
            self.provideNum[0] -= 1
        elif action == 1:
            carId = self.provideBucket[self.px[0]][self.py[0]]
            self.provideBucket[self.px[0]][self.py[0]] = None
            self.provideBucket[0][self.py[0]] = carId
        self.moveInChannel(self.provideBucket, self.py[0])

    #
    # receive car
    #
    def receiveCar(self, carId):
        if self.receiveBucket is None:
            print("Please do CAR.setBucket() first!")
        car = CARDICT[carId]
        leftX = max(min(self.speed_, car.__speed__()) - car.__x__(), 0)
        nextCrossId = self.from_ if car.__nextCrossId__() != self.from_ else self.to_
        if leftX == 0:
            car.updateDynamic(state=2, x=0)
            return 1
        # find front car
        for i in range(self.channel_):
            frontCarLoc = self.findCar(self.length_ - leftX - 1, self.length_ - 1, i, self.receiveBucket)
            # if no front car
            if frontCarLoc == -1:
                self.receiveBucket[self.length_ - leftX][i] = carId
                self.receiveNum[0] += 1
                car.updateDynamic(state=2, x=self.length_ - leftX, y=i, presentRoad=self.id_, roadSpeed=self.speed_,
                                  nextCrossId=nextCrossId)
                return 0
            frontCar = CARDICT[self.receiveBucket[frontCarLoc][i]]
            # if frontCar.state == waiting
            if frontCar.__state__() == 1:
                return 2
            # if frontCar.state == finish and frontCar.x != road.__length__()-1
            elif frontCarLoc != self.length_ - 1:
                self.receiveBucket[frontCarLoc + 1][i] = carId
                self.receiveNum[0] += 1
                car.updateDynamic(state=2, x=frontCarLoc + 1, y=i, presentRoad=self.id_, roadSpeed=self.speed_,
                                  nextCrossId=nextCrossId)
                return 0
            # if frontCar.state == finish and frontCar.x == road.__length__()-1
            else:
                continue
        # if cars' state in all channel is equal to finish
        car.updateDynamic(state=2, x=0)
        return 1

    #
    # show statistic parameters
    #
    def __id__(self):
        return self.id_

    def __length__(self):
        return self.length_

    def __speed__(self):
        return self.speed_

    def __channel__(self):
        return self.channel_

    def __from__(self):
        return self.from_

    def __to__(self):
        return self.to_

    def __isDuplex__(self):
        return self.isDuplex_

    def __carCapcity__(self):
        return self.carCapcity

    #
    # show statistic parameters
    #
    def __forwardBucket__(self):
        return self.forwardBucket

    def __backwardBucket__(self):
        return self.backwardBucket

    def __fx__(self):
        return self.fx[0]

    def __fy__(self):
        return self.fy[0]

    def __bx__(self):
        return self.bx[0]

    def __by__(self):
        return self.by[0]

    def __forwardNum__(self):
        return self.forwardNum[0]

    def __backwardNum__(self):
        return self.backwardNum[0]

    def __forwardDone__(self):
        return self.forwardDone[0]

    def __backwardDone__(self):
        return self.backwardDone[0]

    def __provideBucket__(self):
        return self.provideBucket

    def __receiveBucket__(self):
        return self.receiveBucket

    def __px__(self):
        return self.px[0]

    def __py__(self):
        return self.py[0]

    def __provideNum__(self):
        return self.provideNum[0]

    def __receiveNum__(self):
        return self.receiveNum[0]

    def __provideDone__(self):
        return self.provideDone[0]


class CROSS(object):
    def __init__(self, id_, north_, east_, south_, west_):
        # **** statistic parameters ****#
        self.id_ = id_
        self.roadIds = [north_, east_, south_, west_]
        self.carport = {}
        self.left = []
        # absolute loc
        self.x, self.y = 0, 0
        self.mapX, self.mapY = 0, 0
        # priorityMap
        # 0\1\2 应该代表了车辆转弯的优先级。北向南直行：2，北向东左转：1，北向西右转：0；以此类推
        self.priorityMap = {north_: {east_: 1, south_: 2, west_: 0}, \
                            east_: {south_: 1, west_: 2, north_: 0}, \
                            south_: {west_: 1, north_: 2, east_: 0}, \
                            west_: {north_: 1, east_: 2, south_: 0}}
        # relationship with roads
        # 遍历路口的所有道路，
        # 如果该道路的to是该路口或isDuplex，把道路加入到provideIndex中
        # 如果该道路的from是该路口或isDuplex，把该道路加入到receiverIndex中
        # 如果存在该道路，就加入validRoadIndex中
        # 即来的路（provide）、去的路（receiver）、并都加入valid
        # provider排序，按roadId进行升序调度
        self.providerIndex, self.receiverIndex, self.validRoadIndex = [], [], []
        for index, roadId in enumerate(self.roadIds):
            road = ROADDICT[roadId] if roadId != -1 else None
            if road is not None and (road.__isDuplex__() or road.__to__() == self.id_):
                self.providerIndex.append(index)
            if road is not None and (road.__isDuplex__() or road.__from__() == self.id_):
                self.receiverIndex.append(index)
            if road is not None:
                self.validRoadIndex.append(index)
        self.provider = [self.roadIds[index] for index in self.providerIndex]
        self.receiver = [self.roadIds[index] for index in self.receiverIndex]
        self.validRoad = [self.roadIds[index] for index in self.validRoadIndex]
        self.provider.sort()
        # **** dynamic parameters ****#
        self.readyCars = []
        self.carportCarNum = 0
        self.finishCarNum = 0
        # **** flag ****#
        self.done = False
        self.update = False

    # main functions
    # 路口车辆的调度***
    # 对于provide的第一优先级车辆进行调度
    def step(self):
        self.update = False
        for roadId in self.validRoad:
            ROADDICT[roadId].setBucket(self.id_)
        # data prepare
        nextCarId, nextCar, nextRoad, nextCarPriority = [], [], [], []
        for provideIndex in range(self.provider.__len__()):
            nextCarId.append(ROADDICT[self.provider[provideIndex]].firstPriorityCar())
            # if first priority car exists
            if nextCarId[provideIndex] != -1:
                nextCar.append(CARDICT[nextCarId[provideIndex]])
                nextRoad.append(nextCar[provideIndex].__nextRoad__())
                # nextRoad == -1 => terminal
                if nextRoad[provideIndex] == -1:
                    nextCarPriority.append(2)   #车辆优先级
                else:
                    nextCarPriority.append(self.prority(self.provider[provideIndex], nextRoad[provideIndex]))
            else:
                nextCar.append(-1)
                nextRoad.append(-1)
                nextCarPriority.append(-1)
        # loop
        for provideIndex in range(self.provider.__len__()):
            while nextCar[provideIndex] != -1:
                # same next road and high priority lead to conflict
                provider = ROADDICT[self.provider[provideIndex]]
                for i in range(self.provider.__len__()):
                    # if conflict(same direction and low priority)
                    if nextRoad[i] == nextRoad[provideIndex] and nextCarPriority[i] > nextCarPriority[provideIndex]:
                        break
                #
                if nextRoad[provideIndex] == -1:
                    provider.firstPriorityCarAct(0)
                    self.update = True
                    CARDISTRIBUTION[1] -= 1
                    CARDISTRIBUTION[2] += 1
                else:
                    nextroad_ = ROADDICT[nextRoad[provideIndex]]
                    action = nextroad_.receiveCar(nextCar[provideIndex].__id__())
                    if action == 2:
                        break
                    provider.firstPriorityCarAct(action)
                    self.update = True
                nextCarId[provideIndex] = provider.firstPriorityCar()
                if nextCarId[provideIndex] != -1:
                    nextCar[provideIndex] = CARDICT[nextCarId[provideIndex]]
                    nextRoad[provideIndex] = nextCar[provideIndex].__nextRoad__()
                    # nextRoad == -1 => terminal
                    if nextRoad[provideIndex] == -1:
                        nextCarPriority[provideIndex] = 2
                    else:
                        nextCarPriority[provideIndex] = self.prority(self.provider[provideIndex],
                                                                     nextRoad[provideIndex])
                else:
                    nextCar[provideIndex] = -1
                    nextRoad[provideIndex] = -1
                    nextCarPriority[provideIndex] = -1
        done = True
        for provideIndex in range(self.provider.__len__()):
            if nextCar[provideIndex] != -1:
                done = False
        self.done = done

    def outOfCarport(self):
        self.readyCars = self.left
        self.left = []
        # add code --------------------(分配出来)
        # 
        #------------------------------
        if TIME[0] in self.carport.keys():
            self.carport[TIME[0]].sort()
            self.readyCars.extend(self.carport[TIME[0]])
        if self.readyCars.__len__() == 0:
            return
        # self.readyCars.sort()
        for roadId in self.receiver:
            ROADDICT[roadId].setBucket(self.id_)
        for i in range(self.readyCars.__len__()):
            carId = self.readyCars[i]
            roadId = CARDICT[carId].__nextRoad__()
            road = ROADDICT[roadId]
            if roadId not in self.receiver:
                print("Car(%d).Road(%d) not in cross(%d).function:class.outOfCarport" % (carId, roadId, self.id_))
            act = road.receiveCar(carId)
            if act != 0:
                self.left = self.readyCars[i:]
                break
            # assert act==0, print("Time(%d),Cross(%d),Road(%d),Car(%d) can't pull away from carport"%(TIME[0],self.id_,roadId,carId))
            self.carportCarNum -= 1
            CARDISTRIBUTION[0] -= 1
            CARDISTRIBUTION[1] += 1

    #
    # other functions
    #
    def prority(self, providerId, receiverId):
        return self.priorityMap[providerId][receiverId]

    def setDone(self, bool):
        self.done = bool

    def setLoc(self, x, y):
        self.x, self.y = x, y

    def setMapLoc(self, mapX, mapY):
        self.mapX, self.mapY = mapX, mapY

    def roadDirection(self, roadId):
        if self.roadIds[0] == roadId:
            return 0
        elif self.roadIds[1] == roadId:
            return 1
        elif self.roadIds[2] == roadId:
            return 2
        elif self.roadIds[3] == roadId:
            return 3
        else:
            return -1

    # 每个cross都有个carport，carport里存放timePlan：carId， timePlan为key值。
    def carportInitial(self, timePlan, carId):
        if timePlan not in self.carport.keys():
            self.carport[timePlan] = [carId]
        else:
            self.carport[timePlan].append(carId)
        self.carportCarNum += 1

    #
    # show statistic parameters
    #
    def __id__(self):
        return self.id_

    def __roadIds__(self):
        return self.roadIds

    def __providerIndex__(self):
        return self.providerIndex

    def __receiverIndex__(self):
        return self.receiverIndex

    def __validRoadIndex__(self):
        return self.validRoadIndex

    def __provider__(self):
        return self.provider

    def __receiver__(self):
        return self.receiver

    def __validRoad__(self):
        return self.validRoad

    def __x__(self):
        return self.x

    def __y__(self):
        return self.y

    def __mapX__(self):
        return self.mapX

    def __mapY__(self):
        return self.mapY

    def __done__(self):
        return self.done

    #
    # show dynamic parameters
    #
    def __carportCarNum__(self):
        return self.carportCarNum

    def __finishCarNum__(self):
        return self.finishCarNum

    def __update__(self):
        return self.update

    #
    # show some important info
    #
    def __loc__(self):
        return self.x, self.y

    def __mapLoc__(self):
        return self.mapX, self.mapY

# add by ymb
class GRAPH(object):
    def __init__(self):
        # graph的邻居矩阵
        self.adj_matrix = [[float('inf') for i in range(len(CROSSNAMESPACE))] for i in range(len(CROSSNAMESPACE))]
        # 储存两点之间的最短路径
        self.floyd_min_cross = [[None for i in range(len(CROSSNAMESPACE))] for i in range(len(CROSSNAMESPACE))]
        self.floyd_min_route = [[None for i in range(len(CROSSNAMESPACE))] for i in range(len(CROSSNAMESPACE))]
        # 建立了cross与road之间的联系
        self.establish_adjmatrix()
        # 利用Floyd建立最短路径
        self.floyd_algorithm()
    
    #  
    def establish_adjmatrix(self):
        for crossId in CROSSNAMESPACE:
            cross = CROSSDICT[crossId]
            for roadId in cross.__roadIds__():
                if roadId == -1:
                    continue
                if ROADDICT[roadId].__from__() == crossId:
                    #f = ROADDICT[roadId].__from__()
                    #t = ROADDICT[roadId].__to__()
                    #adj_matrix[f][t] = ROADDICT[roadId].__length__()
                    self.adj_matrix[ROADDICT[roadId].__from__()-1][ROADDICT[roadId].__to__()-1] = ROADDICT[roadId].__length__()
                    if ROADDICT[roadId].__isDuplex__() == 1:
                        self.adj_matrix[ROADDICT[roadId].__to__()-1][ROADDICT[roadId].__from__()-1] = ROADDICT[roadId].__length__()

    def floyd_algorithm(self):
        adj_temp = self.adj_matrix[:]
        for i in range(len(CROSSNAMESPACE)):
            for j in range(len(CROSSNAMESPACE)):
                if i == j:
                    continue
                if adj_temp[i][j]:
                    self.floyd_min_cross[i][j] = [i+1, j+1]

        for i in range(len(CROSSNAMESPACE)):
            for j in range(len(CROSSNAMESPACE)):
                for k in range(len(CROSSNAMESPACE)):
                    if i == j or i == k or j == k:
                        continue
                    if adj_temp[i][j] > adj_temp[i][k] + adj_temp[k][j]:
                        adj_temp[i][j] = adj_temp[i][k] + adj_temp[k][j]
                        self.floyd_min_cross[i][j] = self.floyd_min_cross[i][k] + self.floyd_min_cross[k][j][1:]
                    if adj_temp[i][j] == adj_temp[i][k] + adj_temp[k][j]:
                        ran = random.random()
                        if ran < 0.5:
                            adj_temp[i][j] = adj_temp[i][k] + adj_temp[k][j]
                            self.floyd_min_cross[i][j] = self.floyd_min_cross[i][k] + self.floyd_min_cross[k][j][1:]

        # 将最短路的各个cross点，转化为各个cross之间的road
        for i in range(len(CROSSNAMESPACE)):
            for j in range(len(CROSSNAMESPACE)):
                if i == j:
                    continue
                for k in range(len(self.floyd_min_cross[i][j])):
                    if k+1 < len(self.floyd_min_cross[i][j]):
                        crossId_cur = self.floyd_min_cross[i][j][k]
                        crossId_next = self.floyd_min_cross[i][j][k+1]
                        for roadId in CROSSDICT[crossId_cur].__roadIds__():
                            if roadId == -1:
                                continue
                            if ROADDICT[roadId].__to__() == crossId_cur and ROADDICT[roadId].__from__() == crossId_next:
                                if self.floyd_min_route[i][j] == None:
                                    self.floyd_min_route[i][j] = [roadId]
                                    break
                                else:
                                    self.floyd_min_route[i][j] = self.floyd_min_route[i][j] + [roadId]
                                    break
                            if ROADDICT[roadId].__from__() == crossId_cur and ROADDICT[roadId].__to__() == crossId_next:
                                if self.floyd_min_route[i][j] == None:
                                    self.floyd_min_route[i][j] = [roadId]
                                    break
                                else:
                                    self.floyd_min_route[i][j] = self.floyd_min_route[i][j] + [roadId]
                                    break
    def init_plantime_route(self):
        for carId in CARNAMESPACE:
            car = CARDICT[carId]
            route = self.floyd_min_route[car.__from__()][car.__to__()]
            # lhw需要加入planTime来初始化
            CARDICT[carId].simulateInit(route=route)                            
    def __minRoute__(self, i, j):
        return self.floyd_min_route[i-1][j-1]
                        





class simulation(object):
    def __init__(self):
        self.dead = False

    def step(self):
        print("time:%d" % TIME[0])
        for crossId in CROSSNAMESPACE:
            CROSSDICT[crossId].setDone(False)
        print("pre-movement...")
        for road in ROADNAMESPACE:
            ROADDICT[road].stepInit()
        print("while loop...")
        unfinishedCross = CROSSNAMESPACE
        while unfinishedCross.__len__() > 0:
            self.dead = True
            nextCross = []
            for crossId in unfinishedCross:
                cross = CROSSDICT[crossId]
                cross.step()
                if not cross.__done__():
                    nextCross.append(crossId)
                if cross.__update__() or cross.__done__():
                    self.dead = False
            unfinishedCross = nextCross
            if self.dead:
                print("dead lock in", unfinishedCross)
                return
        print("car pulling away from carport")
        for i in range(CROSSNAMESPACE.__len__()):
            crossId = CROSSNAMESPACE[i]
            for roadId in CROSSDICT[crossId].__validRoad__():
                ROADDICT[roadId].setBucket(crossId)
            CROSSDICT[crossId].outOfCarport()

    def simulate(self):
        visualize = visualization()
        visualize.crossLocGen()
        while True:
            self.step()
            #visualize.drawMap()
            if CARDISTRIBUTION[2] == CARNAMESPACE.__len__():
                print(CARDISTRIBUTION[2])
                break
            if self.dead:
                break
            TIME[0] += 1


class visualization(object):
    def __init__(self):
        self.maxX, self.maxY = 0, 0
        self.savePath = '../../simulatePictures'
        # ** cross param **#
        self.crossRadius = 14
        self.crossDistance = 150
        self.crossColor = [25, 200, 0]
        # ** road param **#
        self.roadColor = [0, 0, 0]  # black
        self.roadLineType = 4
        self.channelWidth = 5
        self.channelDistance = 3
        self.lineWidth = 2
        self.time = 0

    #
    # cross location gen
    #
    def crossLocGen(self):
        # **** relative location ****#
        # denote the first cross as the origin of coordinates
        for crossId in CROSSNAMESPACE:
            CROSSDICT[crossId].setDone(False)
        crossList = [CROSSNAMESPACE[0]]
        minX, minY = 0, 0
        while (crossList.__len__() > 0):
            nextCrossList = []
            for crossId in crossList:
                presentX, presntY = CROSSDICT[crossId].__loc__()
                validRoad = CROSSDICT[crossId].__validRoad__()
                for roadId in validRoad:
                    # next cross id
                    nextCrossId = ROADDICT[roadId].__from__() if ROADDICT[roadId].__from__() != crossId \
                        else ROADDICT[roadId].__to__()
                    # if next cross is visited
                    if not CROSSDICT[nextCrossId].__done__():
                        # visit sets true
                        CROSSDICT[nextCrossId].setDone(True)
                        # relative location of nextcross
                        nextX, nextY = self.crossRelativeLoc(presentX, presntY, crossId, roadId)
                        # update location
                        CROSSDICT[nextCrossId].setLoc(nextX, nextY)
                        minX, minY, self.maxX, self.maxY = \
                            min(nextX, minX), min(nextY, minY), max(nextX, self.maxX), max(nextY, self.maxY)
                        nextCrossList.append(nextCrossId)
            crossList = nextCrossList
        self.maxX, self.maxY = (self.maxX - minX + 2) * self.crossDistance, (self.maxY - minY + 2) * self.crossDistance
        for crossId in CROSSNAMESPACE:
            x, y = CROSSDICT[crossId].__loc__()
            CROSSDICT[crossId].setLoc(x - minX, y - minY)
            CROSSDICT[crossId].setMapLoc((x - minX + 1) * self.crossDistance, (y - minY + 1) * self.crossDistance)

    def crossRelativeLoc(self, x, y, crossId, roadId):
        roadDirection = CROSSDICT[crossId].roadDirection(roadId)
        if roadDirection == 0:
            return x, y - 1
        elif roadDirection == 1:
            return x + 1, y
        elif roadDirection == 2:
            return x, y + 1
        elif roadDirection == 3:
            return x - 1, y
        else:
            print("Cross(%d) don't interact with road(%d)" % (self.id_, roadId))

    #
    # draw functions
    #

    def drawMap(self):
        img = np.ones((self.maxX,self.maxY,3),np.uint8)*255
        #draw road
        for roadId in ROADNAMESPACE:
            self.plotRoad(roadId,img)
        # draw cross
        for crossId in CROSSNAMESPACE:
            self.plotCross(crossId,img)
        # plot info
        self.plotInfo(img)
        cv.imwrite(self.savePath+'/%d.jpg'%TIME[0],img)
    def plotCross(self,crossId,img):
        x, y = CROSSDICT[crossId].__mapLoc__()
        cv.circle(img,(x,y),self.crossRadius,color=self.crossColor,thickness=-1,lineType=-1)
        if crossId>=10:
            xx, yy = int(x - 4*self.crossRadius/5), int(y + self.crossRadius / 2)
        else:
            xx, yy = int(x- self.crossRadius/2), int(y + self.crossRadius / 2)
        cv.putText(img,str(crossId),(xx,yy ),cv.FONT_HERSHEY_SIMPLEX,0.6,[0,0,255],2)
    def plotRoad(self,roadId,img):
        # get road info
        road = ROADDICT[roadId]
        fromX, fromY = CROSSDICT[road.__from__()].__mapLoc__()
        toX, toY = CROSSDICT[road.__to__()].__mapLoc__()
        # plot line
        cv.line(img,(fromX, fromY),(toX, toY),color=self.roadColor,thickness=2)
        # plot bucket
        self.drawBucket(road,'forward',img)
        if road.__isDuplex__():
            self.drawBucket(road,'backward',img)
    def drawBucket(self,road,lane,img):
        bucket = road.__forwardBucket__() if lane !='backward' else road.__backwardBucket__()
        length = road.__length__()
        channel = road.__channel__()
        fromX, fromY = CROSSDICT[road.__from__()].__mapLoc__()
        toX, toY = CROSSDICT[road.__to__()].__mapLoc__()
        XY, intervalXY, rectangleSize, channel2XY, length2XY = self.bucketDrawInitial(fromX,fromY,toX,toY,lane,length)
        for i in range(length):
            for j in range(channel):
                xRD,yRD = int(XY[0]+rectangleSize[0]),int(XY[1]+rectangleSize[1])
                if bucket[i][j] is  None:
                    cv.rectangle(img,(int(XY[0]),int(XY[1])),(xRD,yRD),(0,0,0),1)
                else:
                    color = CARDICT[bucket[i][j]].__carColor__()
                    cv.rectangle(img, (int(XY[0]), int(XY[1])),(xRD, yRD),color=color,thickness=-1)
                XY[channel2XY] = XY[channel2XY] + intervalXY[channel2XY]
            XY[channel2XY] = XY[channel2XY] - intervalXY[channel2XY]*channel
            XY[length2XY] = XY[length2XY] + intervalXY[length2XY]
    def bucketDrawInitial(self,fromX,fromY,toX,toY,lane,length):
        direction = self.bucketDirection(fromX,fromY,toX,toY,lane)
        unitLength = (self.crossDistance - self.crossRadius * 4) / length
        if lane=='backward':
            toY=fromY
            toX=fromX
        if direction == 'north':
            XY = [fromX + self.channelDistance,toY + self.crossRadius * 2]
            intervalXY = self.channelDistance  + self.channelWidth , unitLength
            rectangleSize = self.channelWidth , unitLength
            channel2XY, length2XY = 0, 1
        elif direction == 'south':
            XY = [fromX - self.channelDistance - self.channelWidth,toY - self.crossRadius * 2 - unitLength]
            intervalXY = -(self.channelDistance  + self.channelWidth ), -unitLength
            rectangleSize = self.channelWidth , unitLength
            channel2XY, length2XY = 0, 1
        elif direction == 'east':
            XY = [toX - self.crossRadius * 2 - unitLength,fromY + self.channelDistance]
            intervalXY = -unitLength, self.channelDistance + self.channelWidth
            rectangleSize = unitLength, self.channelWidth
            channel2XY, length2XY = 1, 0
        elif direction == 'west':
            XY = [toX + self.crossRadius * 2, fromY - self.channelDistance - self.channelWidth]
            intervalXY = unitLength, -(self.channelDistance + self.channelWidth)
            rectangleSize = unitLength, self.channelWidth
            channel2XY, length2XY = 1, 0
        return XY,intervalXY,rectangleSize,channel2XY,length2XY
    def bucketDirection(self,fromX,fromY,toX,toY,lane):
        if fromY > toY:
            direction = 'north' if lane=='forward' else 'south'
        elif fromY < toY:
            direction = 'south' if lane == 'forward' else 'north'
        elif fromX < toX:
            direction = 'east' if lane == 'forward' else 'west'
        else:
            direction = 'west' if lane == 'forward' else 'east'
        return direction
    def plotInfo(self,img):
        for crossId in CROSSNAMESPACE:
            cross = CROSSDICT[crossId]
            x,y = cross.__mapLoc__()
            cn,fn = cross.__carportCarNum__(),cross.__finishCarNum__()
            cv.putText(img,"%d,%d"%(cn,fn),(int(x),int(y-1.1*self.crossRadius)),\
                       cv.FONT_HERSHEY_SIMPLEX,0.4,[0,0,255],1)
        cv.putText(img, "in the carport:%d,on the road:%d,end of the trip:%d" % (CARDISTRIBUTION[0],CARDISTRIBUTION[1],CARDISTRIBUTION[2]),(30,30), \
                   cv.FONT_HERSHEY_SIMPLEX, 0.6, [0, 0, 255], 2)


class FILESYSTEM(object):
    def __init__(self, argv1, argv2, argv3, argv4):
        self.car_path = argv1
        self.road_path = argv2
        self.cross_path = argv3
        self.answer_path = argv4
        self.carInfo = []
        self.roadInfo = []
        self.crossInfo = []
        self.carsortinfo = []

        self.loadfile()

    #sort car max v and start time
    def sort_car(self,car=[]):
        car = [list(i) for i in car]
        car_np = np.array(car)
        car_v = car_np.transpose()[-2]
        index = np.argsort(car_v)[::-1]
        car_v_sort = car_np[index]

        car_v_sort_t = car_v_sort.transpose()[-2]
        diff = np.diff(car_v_sort_t)
        index2 = diff.nonzero()[0]
        car_sort_vandt = np.zeros_like(car_np)
        for i in range(index2.shape[0]):
            if i == 0:
                transpose = car_v_sort[0:index2[i]].transpose()[-1]
                index_time = np.argsort(transpose)
                car_sort_vandt[0:index2[i]] = car_v_sort[0:index2[i]][index_time]
                #print(index_time)
                #car_sort_vandt[0:index2[i]] = car_v_sort_t
            else:
                transpose = car_v_sort[index2[i-1]:index2[i]].transpose()[-1]
                index_time = np.argsort(transpose)
                car_sort_vandt[index2[i-1]:index2[i]] = car_v_sort[index2[i-1]:index2[i]][index_time]
        transpose = car_v_sort[index2[-1]:].transpose()[-1]
        index_time = np.argsort(transpose)
        car_sort_vandt[index2[-1]:] = car_v_sort[index2[-1]:][index_time]
        return car_sort_vandt.tolist()
    #sort over

    def start_time_modify(self,carsort=[],epoch=15):
        length = len(carsort)
        cur_time = 1
        for j in range(length):
            if j % epoch == 0 and j > 0:
                cur_time += 1
            if carsort[j][-1] > cur_time:
                cur_time = carsort[j][-1]
            carsort[j][-1] = cur_time
        return carsort
            

    def loadfile(self):
        self.carInfo = open(self.car_path, 'r').read().split('\n')[1:]
        self.roadInfo = open(self.road_path, 'r').read().split('\n')[1:]
        self.crossInfo = open(self.cross_path, 'r').read().split('\n')[1:]
        #self.answerInfo = open(self.answer_path, 'r').read().split('\n')
        # *****************************Create NameSpace And Dictionary*****************************#
        # create car objects
        # line = (id,from,to,speed,planTime)
        '''for line in self.carInfo:
            id_, from_, to_, speed_, planTime_ = line.replace(' ', '').replace('\t', '')[1:-1].split(',')
            CARNAMESPACE.append(int(id_))
            CARDICT[int(id_)] = CAR(int(id_), int(from_), int(to_), int(speed_), int(planTime_))'''
        carlist = []
        for line in self.carInfo:
            line = list(eval(line))
            carlist.append(line)
        #print(carlist)
        carinfo = self.sort_car(carlist)
        self.carsortinfo = self.start_time_modify(carinfo,20)
        
        for line in self.carsortinfo:
            id_, from_, to_, speed_, planTime_ = line
            CARNAMESPACE.append(int(id_))
            CARDICT[int(id_)] = CAR(int(id_), int(from_), int(to_), int(speed_), int(planTime_))
        # create road objects
        # line = (id,length,speed,channel,from,to,isDuplex)
        for line in self.roadInfo:
            id_, length_, speed_, channel_, from_, to_, isDuplex_ = line.replace(' ', '').replace('\t', '')[1:-1].split(',')
            ROADNAMESPACE.append(int(id_))
            ROADDICT[int(id_)] = ROAD(int(id_), int(length_), int(speed_), int(channel_), int(from_), int(to_), int(isDuplex_))
        # create cross objects
        # line = (id,north,east,south,west)
        for line in self.crossInfo:
            id_, north_, east_, south_, west_ = line.replace(' ', '').replace('\t', '')[1:-1].split(',')
            CROSSNAMESPACE.append(int(id_))
            CROSSDICT[int(id_)] = CROSS(int(id_), int(north_), int(east_), int(south_), int(west_))
        # car route initialize
        # line = (id,startTime,route)
        #
        #for i, line in enumerate(answerInfo):
        #    if line.strip() == '':
        #        break
        #    line = line.strip()[1:-1].split(',')
        #    carId = int(line[0])
        #    planTime_ = int(line[1])
        #    route = [int(roadId) for roadId in line[2:]]
        #    CARDICT[carId].simulateInit(planTime_, route)
        CARDISTRIBUTION[0] = CARNAMESPACE.__len__()
        # **** cross initialization ****#
        for carId in CARNAMESPACE:
            CROSSDICT[CARDICT[carId].__from__()].carportInitial(CARDICT[carId].__planTime__(), carId)
    
    def answer_print(self, carSort, graph):
        file_write = open(self.answer_path, 'w')
        for carId in carSort:
            if carId not in CARNAMESPACE:
                print("the carId is not exist")
            file_write.write("(%d, %d, " % (CARDICT[carId].__id__(), CARDICT[carId].__planTime__()))
            route = graph.__minRoute__(CARDICT[carId].__from__(), CARDICT[carId].__to__())
            file_write.write(", ".join(str(s) for s in route))
            file_write.write(")\n")
    
    def __CarSort__(self):
        return self.carsortinfo
def main():
    filesys = FILESYSTEM(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4])
    
    # 这里加发车情况，分配发车的planTime，route可以先不分配。
    # ymb
    # 创建地图（邻接矩阵、两点间最短路）



    
    # ****Initialization ****#
    CARNAMESPACE.sort()
    CROSSNAMESPACE.sort()
    # simulator
    #simulate = simulation()
    #simulate.simulate()


    # add by ymb
    graph = GRAPH()
    # carSort 就车的发车顺序 lhw
    carSort = filesys.__CarSort__()
    carSort = [i[0] for i in carSort]
    filesys.answer_print(carSort, graph)
    #route = graph.__minRoute__(CARDICT[carId].__from__(), CARDICT[carId].__to__())
    #print(", ".join(str(s) for s in route))



if __name__ == "__main__":
    main()

# python simulator.py ../config_11/car.txt ../config_11/road.txt ../config_11/cross.txt ../config_11/answer.txt
